import numpy
from scipy import interpolate
from tumpreprocess import LoadTUMData
import random

path = '/home/ubuntu/user_space/dataset-room1_1024_16/dso/gt_imu.csv'

# def main():
#


def interpolateGt():
    ## 1d interpolate
    time, tum_px, tum_py, tum_pz, tum_q0, tum_q1, tum_q2, tum_q3 = LoadTUMData.loadTumGtInterp()

    f0 = interpolate.interp1d(time, tum_px, kind='cubic')  ###
    f1 = interpolate.interp1d(time, tum_py, kind='cubic')
    f2 = interpolate.interp1d(time, tum_pz, kind='cubic')
    f3 = interpolate.interp1d(time, tum_q0, kind='cubic')
    f4 = interpolate.interp1d(time, tum_q1, kind='cubic')
    f5 = interpolate.interp1d(time, tum_q2, kind='cubic')
    f6 = interpolate.interp1d(time, tum_q3, kind='cubic')

    return f0, f1, f2, f3, f4, f5, f6

def getRandomGroup(time, TUM_px, TUM_py, TUM_pz, tum_accex, tum_accey, tum_accez, tum_angux, tum_anguy, tum_anguz):

    f0, f1, f2, f3, f4, f5, f6 = interpolateGt()

    startIndex = random.randint(0, len(time) - 400)
    startTime = time[startIndex]
    beforeStart = time[startIndex - 1]
    endIndex = startIndex + 400
    endTime = time[endIndex]

    TUM_GtPx0 = f0(startTime)
    TUM_GtPy0 = f1(startTime)
    TUM_GtPz0 = f2(startTime)
    TUM_GtPx1 = f0(beforeStart)
    TUM_GtPy1 = f1(beforeStart)
    TUM_GtPz1 = f2(beforeStart)
    TUM_GtPx2 = f0(endTime)
    TUM_GtPy2 = f1(endTime)
    TUM_GtPz2 = f2(endTime)
    TUM_GtQ0 = f3(startTime)
    TUM_GtQ1 = f4(startTime)
    TUM_GtQ2 = f5(startTime)
    TUM_GtQ3 = f6(startTime)

    TUM_GtPQ = []
    for i in range(startIndex, startIndex + 400):
        curTime = time[i]
        f0_time = f0(curTime)
        f1_time = f1(curTime)
        f2_time = f2(curTime)
        f3_time = f3(curTime)
        f4_time = f4(curTime)
        f5_time = f5(curTime)
        f6_time = f6(curTime)
        TUM_GtPQ.append([f0_time, f1_time, f2_time, f3_time, f4_time, f5_time, f6_time])

    Data_Px, Data_Py, Data_Pz = [], [], []
    Tum_accex, Tum_accey, Tum_accez, Tum_angux, Tum_anguy, Tum_anguz = [], [], [], [], [], []
    print([startIndex, endIndex])
    print(len(TUM_px))
    for i in range(startIndex, endIndex + 1):
        # Data_Px.append(TUM_px[i])
        # Data_Py.append(TUM_py[i])
        # Data_Pz.append(TUM_pz[i])
        Tum_accex.append(tum_accex[i])
        Tum_accey.append(tum_accey[i])
        Tum_accez.append(tum_accez[i])
        Tum_angux.append(tum_angux[i])
        Tum_anguy.append(tum_anguy[i])
        Tum_anguz.append(tum_anguz[i])

    getGroup = [Data_Px, Data_Py, Data_Pz,
           [TUM_GtPx0, TUM_GtPy0, TUM_GtPz0],
           [TUM_GtPx1, TUM_GtPy1, TUM_GtPz1],
           [TUM_GtPx2, TUM_GtPy2, TUM_GtPz2],
           [TUM_GtQ0, TUM_GtQ1, TUM_GtQ2, TUM_GtQ3],
                Tum_accex, Tum_accey, Tum_accez, Tum_angux, Tum_anguy, Tum_anguz, TUM_GtPQ]

    print(getGroup[3][0])

    return getGroup